import lejos.hardware.Button;
import lejos.hardware.Sound;
import lejos.hardware.lcd.LCD;
import lejos.hardware.motor.NXTRegulatedMotor;
import lejos.hardware.port.Port;
import lejos.hardware.sensor.EV3ColorSensor;
import lejos.robotics.SampleProvider;
import lejos.utility.Delay;

/**
 * Subsistema de lancamento das bolas.
 * 
 * @author SEK 2014
 * 
 */
public class Lancamento2 implements Runnable {

	// Constantes
	private static final int REPOUSO = 0;
	private static final int ATUALIZANDO = 1;
	private static final int LANCANDO = 2;
	private static final int AJUSTANDO = 3;

	public final boolean DIREITA = false;
	public final boolean ESQUERDA = true;

	public int velBatida;
	private static final int velRetorno = 80;
	private static final int angBatida = 80;
	private static final int angRetorno = 40;
//	private static final int angInvercao = 77;
	
	private static int angEsquerda;
	private static int angDireita;

	public boolean fim = false;

	// Componentes do robo
	private EV3ColorSensor sensorCor;
	private NXTRegulatedMotor motorTaco;

	// Variaveis de estado
	private boolean temBolinha;
	private boolean bolaAzul;
	private boolean bolaLancada;
	private int SubEstado;
	private boolean posicaoTaco;

	/**
	 * Metodo get para a variavel posicaoTaco
	 * 
	 * @param null
	 * @return variavel posicaoTaco
	 */
	public boolean getPosicaoTaco() {
		return this.posicaoTaco;
	}

	/**
	 * Metodo get para a variavel temBolinha
	 * 
	 * @param null
	 * @return variavel temBolinha
	 */
	public boolean getTemBolinha() {
		return this.temBolinha;
	}

	/**
	 * Metodo get para a variavel bolaAzul
	 * 
	 * @param null
	 * @return variavel bolaAzul
	 */
	public boolean getBolaAzul() {
		return this.bolaAzul;
	}

	/**
	 * Metodo get para a variavel bolaLançada
	 * 
	 * @param null
	 * @return variavel bolaLancada
	 */
	public boolean getBolaLancada() {
		return this.bolaLancada;
	}

	/**
	 * Constroi um subsistema de lancamento do robo
	 * 
	 * @param s1
	 *            Porta de ligacao do sensor de cor
	 * @param m1
	 *            Porta de ligacao do motor do taco
	 * @return null
	 */
	public Lancamento2(Port s1, Port m1) {
		this.sensorCor = new EV3ColorSensor(s1);
		this.motorTaco = new NXTRegulatedMotor(m1);

		this.bolaAzul = false;
		this.temBolinha = false;
		this.bolaLancada = true;
		this.posicaoTaco = DIREITA;
		this.SubEstado = REPOUSO;

		motorTaco.suspendRegulation();
		Sound.beepSequenceUp();
		
		LCD.drawString("Ajuste do taco:", 1, 1);
		while (!Button.ENTER.isDown()) {
			Delay.msDelay(100);
			LCD.clear(3);
			
			LCD.drawInt(motorTaco.getTachoCount(), 1, 3);
			
			if(Button.LEFT.isDown()){
				posicaoTaco = ESQUERDA;
				angEsquerda = motorTaco.getTachoCount();
				//motorTaco.resetTachoCount();
			}
			
			if(Button.RIGHT.isDown()){
				posicaoTaco = DIREITA;
				angDireita = motorTaco.getTachoCount();
			//	motorTaco.resetTachoCount();
			}
			
//			if (motorTaco.getTachoCount() < 0) {
//				posicaoTaco = ESQUERDA;
//				LCD.drawString("ESQUERDA", 1, 3);
//			} else {
//				posicaoTaco = DIREITA;
//				LCD.drawString("DIREITA", 1, 3);
//			}
		}
//	    motorTaco.resetTachoCount();
		LCD.clear();
	}

	/**
	 * Metodo no qual o subsistema de lancamento executara suas rotinas de
	 * amostragem do sensor de cor e atualizacao das variaveis correspondentes
	 */
	@Override
	public void run() {
		// TODO Auto-generated method stub
		while (!fim) {
			amostrarSensorCor();
			Thread.yield();
			Delay.msDelay(1000);
		}
	}

	/**
	 * Metodo de lancamento de bolas laranjas
	 * 
	 * @param null
	 * @return Sucesso ou nao do lancamento
	 */
	public boolean lancarBolaLaranja() {
		while (SubEstado != REPOUSO)
			;
		motorTaco.resetTachoCount();
		SubEstado = LANCANDO;
		motorTaco.setSpeed(velBatida);
		if (posicaoTaco == DIREITA) {
			motorTaco.rotate(-angBatida); //-angBatida
			Delay.msDelay(500);
			motorTaco.setSpeed(velRetorno);
			motorTaco.rotate(-angRetorno);      //-angRetorno
			Delay.msDelay(100);
		} else { // if(posicaoTaco == ESQUERDA)
			motorTaco.rotate(angBatida);    //angBatida
			Delay.msDelay(500);
			motorTaco.setSpeed(velRetorno);
			motorTaco.rotate(angRetorno); //angRetorno
			Delay.msDelay(100);
		}
		bolaLancada = true;
		SubEstado = REPOUSO;
		//motorTaco.resetTachoCount();
		return true;
	}

	/**
	 * Metodo de lancamento de bolas azuis
	 * 
	 * @param null
	 * @return Sucesso ou nao do lancamento
	 */
	public boolean lancarBolaAzul(boolean direcao) {
		while (SubEstado != REPOUSO)
			;
		SubEstado = LANCANDO;
		motorTaco.setSpeed(velRetorno);
		if (direcao == DIREITA) {
			motorTaco.rotateTo(angRetorno);
		} else { // if(direcao == ESQUERDA)
			motorTaco.rotateTo(-angRetorno);
		}
		bolaLancada = true;
		SubEstado = REPOUSO;
		motorTaco.resetTachoCount();
		return true;
	}

	/**
	 * Metodo que efetua a leitura e tratamento do sensor de cor. Ocorre tambem
	 * a atualizacao das variaveis correspondentes.
	 * 
	 * @param null
	 * @return Sucesso ou nao da leitura do sensor
	 */
	private boolean amostrarSensorCor() {
		while (SubEstado != REPOUSO)
			;
		SubEstado = ATUALIZANDO;
		SampleProvider coresRGB = sensorCor.getRGBMode();
		float[] amostras = new float[coresRGB.sampleSize()];
		coresRGB.fetchSample(amostras, 0);

		System.out.println(amostras[2]);

		if (amostras[0] < 0.004) {
			temBolinha = false;
		} else if (amostras[0] >= 0.007) {
			bolaAzul = false;
			temBolinha = true;
			bolaLancada = false;
		}
		SubEstado = REPOUSO;
		return true;
	}

	/**
	 * Metodo que ajusta a posicao do taco de acordo com a orientacao do robo
	 * 
	 * @param pos
	 *            posicao na qual o taco deve ser posicionado
	 * @return Sucesso ou nao do reposicionamento
	 */
	public void ajustarTaco(boolean pos) {
		while (SubEstado != REPOUSO)
			;
		SubEstado = AJUSTANDO;
		motorTaco.setSpeed(velRetorno);
		if (pos == DIREITA) {
			motorTaco.rotateTo(angDireita); //-angInversao
		} else { // if(pos == ESQUERDA)
			motorTaco.rotateTo(angEsquerda);
		}
		//motorTaco.resetTachoCount();
		posicaoTaco = pos;
		SubEstado = REPOUSO;
	}

	public void setVelocidadeBatida(int velBatida) {
		this.velBatida = velBatida;
	}
}